from onboardDevSDK.utils.geographic_coordinate_conversion import *
from onboardDevSDK.utils.util import *
from onboardDevSDK.utils.rotation_transform import *


def test1():
    gps1 = (30.7295, 103.9643, 200)
    pos1 = cc_geodetic2ecef(gps1)
    print(pos1)


def test2():
    origin = (30.7295, 103.9645, 100)
    gps = (30.7295, 103.9643, 200)

    dis1 = calculate_haversin_distance(origin, gps)
    dis2 = calculate_3d_distance(origin, gps)
    print(dis1, dis2)


def test3():
    euler = (-1.0, -0.4, 0.0)
    # Rbe = euler2Rbe(euler)
    # euler = Rbe2euler(Rbe)
    ###
    Rbe = euler2Reb(euler)
    euler = Reb2euler(Rbe)
    print(Rbe)
    print(euler)


def main():
    test3()


if __name__ == '__main__':
    main()
